/*
 *  BCWorldObserver.cpp
 *  roborobo-online
 *
 *  Created by Antonio Gonzalez on 29/04/10.
 *
 */

#include "BirthClinic/include/BirthClinic.h"
#include "BirthClinic/include/Agents/BCAgent.h"
#include "World/World.h"

BirthClinic::BirthClinic(World *__world) :
		WorldObserver(__world) {
	_world = __world;

	_world->setInitializeAgents(false);
	_world->setSetUpConnections(false);

	if (loadFiles() == false) {
		std::cout << "[critical] cannot load image files." << std::endl;
		exit(1);
	}

	_nbOfCreatedOrganism = 0;
	_numComponents = 0;
}

BirthClinic::~BirthClinic() {
	// nothing to do.
}

bool BirthClinic::loadFiles() {
	bool returnValue = true;
	BCSharedData::_gMainAgentMaskImage = load_image(
			BCSharedData::_gMainAgentMaskImageFilename);
	if (BCSharedData::_gMainAgentMaskImage == NULL) {
		std::cerr << "Could not load main agent mask image\n";
		returnValue = false;
	}

	BCSharedData::_gComponentAgentMaskImage = load_image(
			BCSharedData::_gComponentAgentMaskImageFilename);
	if (BCSharedData::_gComponentAgentMaskImage == NULL) {
		std::cerr << "Could not load component agent mask image\n";
		returnValue = false;
	}

	return returnValue;
}
void BirthClinic::reset() {
	initWorld();
}

void BirthClinic::step() {
	_step++;

	if (isEmpty()) {
		int numOfReceivedGenotypes = _receivedGenotypes.size();
		if (numOfReceivedGenotypes > 0) {

			BCGenotype* genParent1 = _receivedGenotypes.at(0);
			BCGenotype* genParent2 = _receivedGenotypes.at(1);

			_parentsId.erase(_parentsId.begin(), _parentsId.begin() + 2);
			_receivedGenotypes.erase(_receivedGenotypes.begin(),
					_receivedGenotypes.begin() + 2);

			BCAgentPtr agent = boost::make_shared<BCAgent>(_world, true);

			agent->unregisterAgent();

			double xPos = BCSharedData::_birthClinicCenterPosX;
			double yPos = BCSharedData::_birthClinicCenterPosY;

			agent->setCoord((int) xPos, (int) yPos);
			agent->setCoordReal((int) xPos, (int) yPos);
			agent->buildGenotype(genParent1, genParent2);

			std::vector<BCAgentPtr> agentsToAdd;

			agent->initiateOrganism();

			agentsToAdd.push_back(agent);

			int genotypeLength = agent->genotypeSize();

			Coord2d referenceCoords = agent->getPosition();

			for (int j = 1; j < genotypeLength; j++) {
				BCAgentPtr component = boost::make_shared<BCAgent>(_world,
						false);

				BCGene gene = agent->getGene(j-1);
				int to = gene.getTo();
				Coord2d shift = agent->getShift(to);

				Coord2d newPosition;
				newPosition._x = referenceCoords._x + shift._x * gAgentWidth
						+ 0.5;
				newPosition._y = referenceCoords._y + shift._y * gAgentHeight
						+ 0.5;

				component->setCoord((int) newPosition._x, (int) newPosition._y);
				component->setCoordReal(newPosition._x, newPosition._y);

				agent->getOrganism()->addRobot(component);
				component->setOrganism(agent->getOrganism());
				component->registerAgent();

				agentsToAdd.push_back(component);

				int id = component->getId();
				agent->addComponent(id);
			}


			int numAgents = agentsToAdd.size();

			for (int j = 0; j < numAgents; j++) {
				_world->addAgent(
						boost::dynamic_pointer_cast<RobotAgent>(
								agentsToAdd.at(j)));
			}
		} else if (_nbOfCreatedOrganism < BCSharedData::_gNbOfOrganism) {
			createRandomOrganism();
			_nbOfCreatedOrganism++;
		}
	}

	updateWorld();
}

void BirthClinic::initWorld() {
	if (gUseOrganisms) {
		Organism::reset();
	}
}

void BirthClinic::createRandomOrganism() {
	bool wellCreated = true;

	do {
		std::vector<BCAgentPtr> agentsToAdd;

		BCAgentPtr agent = boost::make_shared<BCAgent>(_world, true);

		double xPos = BCSharedData::_birthClinicCenterPosX;
		double yPos = BCSharedData::_birthClinicCenterPosY;

		agent->setCoord((int) xPos, (int) yPos);
		agent->setCoordReal((int) xPos, (int) yPos);

		agent->buildGenotype();
		agent->initiateOrganism();

		agentsToAdd.push_back(agent);

		int genotypeLength = agent->genotypeSize();

		Coord2d referenceCoords = agent->getPosition();

		for (int j = 1; j < genotypeLength; j++) {
			BCAgentPtr component = boost::make_shared<BCAgent>(_world, false);

			Coord2d shift = agent->getShift(j + 1);
			Coord2d newPosition;
			newPosition._x = referenceCoords._x + shift._x * gAgentWidth + 0.5;
			newPosition._y = referenceCoords._y + shift._y * gAgentHeight + 0.5;

			component->setCoord((int) newPosition._x, (int) newPosition._y);
			component->setCoordReal(newPosition._x, newPosition._y);

			agent->getOrganism()->addRobot(component);
			component->setOrganism(agent->getOrganism());
			component->registerAgent();

			agentsToAdd.push_back(component);
			int id = component->getId();
			agent->addComponent(id);
		}
		std::cout << "--------------------------" << std::endl;

		if (wellCreated) {
			int numAgents = agentsToAdd.size();

			for (int j = 0; j < numAgents; j++) {
				_world->addAgent(
						boost::dynamic_pointer_cast<RobotAgent>(
								agentsToAdd.at(j)));
			}
		} else {
			std::cout << "there was any error and this organism is not valid"
					<< std::endl;
			agentsToAdd.clear();
		}

	} while (!wellCreated);
}

bool BirthClinic::isEmpty() {
	int numAgents = gWorld->getNbOfAgent();

	Coord2d birthClinicTopLeftCorner;
	birthClinicTopLeftCorner._x = BCSharedData::_birthClinicCenterPosX - BCSharedData::_birthClinicWidth / 2;
	birthClinicTopLeftCorner._y = BCSharedData::_birthClinicCenterPosY - BCSharedData::_birthClinicHeight / 2;

	Coord2d birthClinicTopRightCorner;
	birthClinicTopRightCorner._x = BCSharedData::_birthClinicCenterPosX  + BCSharedData::_birthClinicWidth / 2;
	birthClinicTopRightCorner._y = BCSharedData::_birthClinicCenterPosY - BCSharedData::_birthClinicHeight / 2;

	Coord2d birthClinicBottomLeftCorner;
	birthClinicBottomLeftCorner._x = BCSharedData::_birthClinicCenterPosX - BCSharedData::_birthClinicWidth / 2;;
	birthClinicBottomLeftCorner._y = BCSharedData::_birthClinicCenterPosY + BCSharedData::_birthClinicHeight / 2;

	for (int i = 0; i < numAgents; i++) {
		BCAgentPtr agent = boost::dynamic_pointer_cast<BCAgent>(
				gWorld->getAgent(i));

		Coord2d agentPos = agent->getPosition();

		Coord2d coords;
		for (int i = 0; i < 4; i++) {
			switch (i) {
			case 0: // Top Left Corner
				coords._x = agentPos._x - gAgentWidth / 2;
				coords._y = agentPos._y - gAgentHeight / 2;
				break;
			case 1: // Top Right Corner
				coords._x = agentPos._x + gAgentWidth / 2;
				coords._y = agentPos._y - gAgentHeight / 2;
				break;
			case 2: // Bottom Left Corner
				coords._x = agentPos._x - gAgentWidth / 2;
				coords._y = agentPos._y + gAgentHeight / 2;
				break;
			case 3: // Bottom Right Corner
				coords._x = agentPos._x + gAgentWidth / 2;
				coords._y = agentPos._y + gAgentHeight / 2;
				break;
			}


			if ((coords._x >= birthClinicTopLeftCorner._x
					&& coords._x <= birthClinicTopRightCorner._x)
					&& (coords._y >= birthClinicTopLeftCorner._y
					&& coords._y <= birthClinicBottomLeftCorner._y)) {
				return false;
			}
		}
	}

	return true;
}

void BirthClinic::updateWorld() {

	std::vector<int> mainIds = getMainOrganismIds();
	int size = mainIds.size();

	for (int i = 0; i < size - 1; i++){
		int r = i + (rand() % (size - i)); // Random remaining position.
		int tmp = mainIds[i];
		mainIds[i] = mainIds[r];
		mainIds[r] = tmp;
	}

	for (int i = 0; i < size; i++) {

		int id = mainIds.at(i);
		BCAgentPtr agent = boost::dynamic_pointer_cast<BCAgent>(
				gWorld->getAgentById(id));

		Point2d agentPos = agent->getWorldModel()->getPosition();

		if (agent->getState() == ADULT && agent->canReproduce()){
			std::map<int, int> candidates;

			for (int j = 0; j < size; j++) {
				int otherId = mainIds.at(j);

				if(otherId != id){

					BCAgentPtr other = boost::dynamic_pointer_cast<BCAgent>(
							gWorld->getAgentById(otherId));

					if (other->getState() == ADULT && other->canReproduce()) {

						int distance = agentPos.distance(
								other->getWorldModel()->getPosition());

						if (distance <= BCSharedData::_matingViewRange) {
							int fitValue = other->evaluate();
							int id = other->getId();
							candidates[id] = fitValue;
						}
					}
				}
			}

			if(!candidates.empty()){

				int selectedId = proportionalSelection(candidates);

				int agentIncluded = getGenotypePosition(
						agent->getId());
				int otherIncluded = getGenotypePosition(
						selectedId);

				if (agentIncluded == -1 && otherIncluded == -1) {
					BCAgentPtr selected = boost::dynamic_pointer_cast<BCAgent>(
							gWorld->getAgentById(selectedId));

					addGenotype(agent->getGenotype(),
							agent->getId());
					addGenotype(selected->getGenotype(),
							selectedId);

					agent->startCooldownPeriod();
					selected->startCooldownPeriod();
				}
			}
		}
	}

	int step = gWorld->getIterations();

	if(step%BCSharedData::_statisticsInterval == 0){
		writeStatistics();
	}
}

int BirthClinic::getGenotypePosition(int id_) {

	int size = _parentsId.size();

	for (int i = 0; i < size; i++) {
		if (_parentsId.at(i) == id_) {
			return i;
		}
	}

	return -1;
}

bool BirthClinic::addGenotype(BCGenotype* gen_, int parentId_) {
	int agentContained = getGenotypePosition(parentId_);
	if (agentContained == -1) {
		_receivedGenotypes.push_back(gen_);
		_parentsId.push_back(parentId_);
		return true;
	}

	return false;
}

void BirthClinic::printReceivedGenomes(){
	int receivedGen = _parentsId.size();

	std::cout << "-----------------------" << std::endl;
	for(int i = 0; i < receivedGen ; i=i+2){
		int parent1 = _parentsId.at(i);
		int parent2 = _parentsId.at(i+1);

		std::cout << "Parent 1: " << parent1 << "\tParent 2: " << parent2 << std::endl;
	}

	std::cout << "-----------------------" << std::endl;
}

std::vector<int> BirthClinic::getMainOrganismIds(){

	std::vector<int> ids;

	int numAgents = gWorld->getNbOfAgent();

	for(int i = 0; i<numAgents; i++){
		BCAgentPtr agent = boost::dynamic_pointer_cast<BCAgent>(
						gWorld->getAgent(i));

		if(agent->isMainAgent()){
			int id = agent->getId();
			ids.push_back(id);
		}
	}
	return ids;
}

int BirthClinic::proportionalSelection(std::map<int, int> list_){

	std::map<int,int>::iterator it;
	int max = 0;
	for (it = list_.begin(); it != list_.end(); ++it){
		max += it->second;
	}

	int random = rand() % max;

	int aux = 0;

	it = list_.begin();
	bool found = false;

	int selected = -1;

	do{
		aux += it->second;

		if(aux >= random){
			selected = it->first;
			found = true;
		}
		++it;
	}
	while(it!=list_.end() && !found);

	return selected;
}

void BirthClinic::writeStatistics(){
	int step = gWorld->getIterations();

	if(step==0){
		BCSharedData::_birthClinicLogFile << "step\tnumOrganisms\tmeanSize\tmaxSize\tminSize\tmeanAge\tmaxAge\tminAge"<< std::endl;
	}

	std::vector<int> ids = getMainOrganismIds();

	int numOrganisms = 0;

	int maxAge = 0;
	int minAge = 0;
	int cumulativeAge = 0;

	int maxSize = 0;
	int minSize = 0;
	int cumulativeSize = 0;

	if(!ids.empty()){
		int size = ids.size();
		for(int i = 0; i< size; i++){
			int id = ids.at(i);
			BCAgentPtr agent = boost::dynamic_pointer_cast<BCAgent>(
							gWorld->getAgentById(id));

			int agentSize = agent->getSize();

			if(agentSize !=-1){
				numOrganisms++;
				cumulativeAge += agent->getAge();
				cumulativeSize += agentSize;

				if(i==0){
					maxAge = agent->getAge();
					minAge = agent->getAge();

					maxSize = agentSize;
					minSize = agentSize;
				}
				else{

					if(agent->getAge() > maxAge){
						maxAge = agent->getAge();
					}
					else if(agent->getAge() < minAge){
						minAge = agent->getAge();
					}

					if(agent->getSize() > maxSize){
						maxSize = agent->getSize();
					}
					else if(agent->getSize() < minSize){
						minSize = agent->getSize();
					}
				}
			}
		}

		if(numOrganisms > 0){
		BCSharedData::_birthClinicLogFile << step << "\t" << numOrganisms << "\t" <<
				((double)cumulativeSize/numOrganisms) << "\t" << maxSize << "\t" << minSize
				 << "\t" << ((double)cumulativeAge/numOrganisms) << "\t" << maxAge << "\t" << minAge << std::endl;
		}
		else{
			BCSharedData::_birthClinicLogFile << step << "\t" << 0 << "\t" <<
									0 << "\t" << 0 << "\t" << 0
									 << "\t" << 0 << "\t" << 0 << "\t" << 0 << std::endl;
		}
	}
	else{
		BCSharedData::_birthClinicLogFile << step << "\t" << 0 << "\t" <<
						0 << "\t" << 0 << "\t" << 0
						 << "\t" << 0 << "\t" << 0 << "\t" << 0 << std::endl;
	}
}
